import operator
import numpy as np
import warnings
from Source.CWJ.guandaohuafen import *
def dian2(p1,p2,width,height):#第二种即两个中心点z不一样
    u=0
    p3=np.zeros(3)
    p4=np.zeros(3)
    p5=np.zeros(3)
    p6=np.zeros(3)
    p_1=np.zeros(3)
    p_2=np.zeros(3)
    p_3=np.zeros(3)
    p_4=np.zeros(3)
    p_5=np.zeros(3)
    p_6=np.zeros(3)
    p_7=np.zeros(3)
    p_8=np.zeros(3)
    if p1[1]-p2[1]==0:
        u=1
    if u==0:
        k=-(p1[0]-p2[0])/(p1[1]-p2[1])
        x=width/(2*(1+k**2)**0.5)
        p3[0]=p1[0]+x
        p3[1]=p1[1]+k*x
        p3[2]=p1[2]
        p4[0]=p1[0]-x
        p4[1]=p1[1]-k*x
        p4[2]=p1[2]
        p5[0]=p2[0]+x
        p5[1]=p2[1]+k*x
        p5[2]=p2[2]
        p6[0]=p2[0]-x
        p6[1]=p2[1]-k*x
        p6[2]=p2[2]
    else :
        p3[0]=p1[0]
        p3[1]=p1[1]+width/2
        p3[2]=p1[2]
        p4[0]=p1[0]
        p4[1]=p1[1]-width/2
        p4[2]=p1[2]
        p5[0]=p2[0]
        p5[1]=p2[1]+width/2
        p5[2]=p2[2]
        p6[0]=p2[0]
        p6[1]=p2[1]-width/2
        p6[2]=p2[2]
    a=(p2[1]-p1[1])*(p3[2]-p1[2])-(p3[1]-p1[1])*(p2[2]-p1[2])
    b=(p2[2]-p1[2])*(p3[0]-p1[0])-(p3[2]-p1[2])*(p2[0]-p1[0])
    c=(p2[0]-p1[0])*(p3[1]-p1[1])-(p3[0]-p1[0])*(p2[1]-p1[1])
    t=height/(2*(a**2+b**2+c**2)**0.5)
    p_3[0]=p3[0]+a*t
    p_3[1]=p3[1]+b*t
    p_3[2]=p3[2]+c*t
    p_2[0]=p3[0]-a*t
    p_2[1]=p3[1]-b*t
    p_2[2]=p3[2]-c*t
    p_4[0]=p4[0]+a*t
    p_4[1]=p4[1]+b*t
    p_4[2]=p4[2]+c*t
    p_1[0]=p4[0]-a*t
    p_1[1]=p4[1]-b*t
    p_1[2]=p4[2]-c*t
    p_7[0]=p5[0]+a*t
    p_7[1]=p5[1]+b*t
    p_7[2]=p5[2]+c*t
    p_6[0]=p5[0]-a*t
    p_6[1]=p5[1]-b*t
    p_6[2]=p5[2]-c*t
    p_8[0]=p6[0]+a*t
    p_8[1]=p6[1]+b*t
    p_8[2]=p6[2]+c*t
    p_5[0]=p6[0]-a*t
    p_5[1]=p6[1]-b*t
    p_5[2]=p6[2]-c*t
    list = [p_3,p_7,p_6,p_2,p_4,p_8,p_5,p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new


# 将障碍物信息从坐标转换成起点终点、宽、高的形式
def block_to_block1(a):
    d = []
    for i in range(len(a)):
        a0 = a[i][0]
        a1 = a[i][1]
        b = [a0[0],a0[1]+a1[1]/2,a0[2]+a1[2]/2]
        b1 = [b[0]+a1[0],b[1],b[2]]
        c = [b,b1,a1[1],a1[2]]
        d.append(c)
    # print(d)
    return d

# pi = [[[100, 1000, 1000], [150.0, 1000, 1000], 100, 100], [[150.0, 1000, 1000], [150.0, 875.0, 1000.0], 100, 100], [[150.0, 875.0, 1000.0], [600.0, 875.0, 1000.0], 100, 100], [[600.0, 875.0, 1000.0], [600, 500, 1000], 100, 100], [[600, 500, 1000], [750.0, 500, 1000], 100, 100], [[750.0, 500, 1000], [750.0, 500, 875.0], 100, 100], [[750.0, 500, 875.0], [1200, 500, 875.0], 100, 100]]
# x = 100
#将一条管道扩列成障碍物
def pipes_to_blocks(pipes,x):
    d = []
    for i in range(len(pipes)):
        a = pipes[i]
        a0 = a[0]
        a1 = a[1]
        a2 = a[2]
        a3 = a[3]
        # a4 = a[4]
        b0 = a2 + x * 2
        b1 = a3 + x * 2
        b = [a0, a1, b0, b1]
        d.append(b)
    # print('dddddddddddddddddddddddddddddddddddddddd',d)
    # [[[-258726.713113349, -6703.050007057, 21636.898155698], [-258726.713021555, -6703.050007057, 14540.971124858059], 85.0, 85.0], [[-279290.027320597, -6703.050007057, 14472.37112485806], [-258795.313021555, -6703.050007057, 14267.42398186764], 85.0, 85.0], [[-279358.627320597, -6634.450007057, 14267.42398186764], [-279358.627320597, -3139.669551513, 14232.476177312199], 85.0, 85.0], [[-279358.627320597, -3071.069551513, 14163.876177312199], [-279358.627320597, -3071.069551513, 9128.705051532199], 85.0, 85.0], [[-279123.453023127, -3071.069551513, 9060.105051532199], [-279290.027320597, -3071.069551513, 9058.4393085575], 85.0, 85.0], [[-279054.853023127, -3002.469551513, 9058.4393085575], [-279054.853023127, -1740.549036018, 9045.82010340255], 85.0, 85.0], [[-277270.754208943, -1671.949036018, 9045.82010340255], [-278986.253023127, -1671.949036018, 9028.66511526071], 85.0, 85.0], [[-277202.154208942, -1374.669551514, 9028.66511526071], [-277202.154208942, -1603.349036018, 9026.37832041567], 85.0, 85.0], [[-277202.154208942, -1306.069551514, 8957.77832041567], [-277202.154208943, -1306.069551514, 7476.378320420662], 85.0, 85.0]]
    return d
# pipes_to_blocks(pi,x)

#多条管道和多个障碍物
# P = [[[100, 1000, 1000], [600, 1000, 1000], 100, 100], [[600, 1000, 1000], [600, 500, 1000], 100, 100], [[600, 500, 1000], [1200, 500, 1000], 100, 100]]
# bs =  [[[300, 1000, 0], [300, 1000, 1200], 100, 100], [[300, 700, 0], [300, 700, 1000], 100, 100],[[900, 100, 1000], [900, 1200, 1000], 100, 100]]
# area = [[[168534.776042965, 170203.809211494, 54568.2], [173584.776042965, 172353.809211494, 56718.2]], [[168534.776042965, 170203.809211494, 54568.2], [173584.776042965, 172353.809211494, 56718.2]], [[168534.776042965, 170203.809211494, 54568.2], [173584.776042965, 172353.809211494, 56718.2]], [[168534.776042965, 168503.809211494, 54568.2], [173584.776042965, 170653.809211494, 56718.2]], [[168534.776042965, 168503.809211494, 54568.2], [173584.776042965, 170653.809211494, 56718.2]], [[168534.776042965, 168503.809211494, 54568.2], [173584.776042965, 170653.809211494, 56718.2]], [[168534.776042965, 166903.809211494, 54568.2], [173584.776042965, 169053.809211494, 56718.2]], [[168534.776042965, 166903.809211494, 54568.2], [173584.776042965, 169053.809211494, 56718.2]], [[168534.776042965, 166903.809211494, 54568.2], [173584.776042965, 169053.809211494, 56718.2]], [[203625.224403649, 169901.85355678, 54518.3], [210623.224403649, 172151.85355678, 56768.3]], [[203625.224403649, 169901.85355678, 54518.3], [210623.224403649, 172151.85355678, 56768.3]], [[203625.224403649, 167382.519568495, 54568.3], [213421.224403649, 169532.519568495, 56718.3]], [[203625.224403649, 167382.519568495, 54568.3], [213421.224403649, 169532.519568495, 56718.3]], [[203625.224403649, 154409.614647389, 54593.3], [210323.224403649, 156509.614647389, 56693.3]], [[203625.224403649, 151837.121221619, 54543.3], [210323.224403649, 154037.121221619, 56743.3]], [[203625.224403649, 151837.121221619, 54543.3], [210323.224403649, 154037.121221619, 56743.3]], [[203625.224403649, 147809.562387894, 54593.3], [210360.197174243, 149909.562387894, 56693.3]], [[203625.224403649, 147809.562387894, 54593.3], [210360.197174243, 149909.562387894, 56693.3]], [[208656.224403649, 154409.614647389, 54593.3], [213454.224403649, 156509.614647389, 56693.3]], [[203625.224403649, 144195.504744438, 54543.3], [208643.224403649, 146395.504744438, 56743.3]], [[207007.224403649, 144195.504744438, 54543.3], [212605.224403649, 146395.504744438, 56743.3]], [[203627.224403649, 142186.634469268, 54543.3], [212323.224403649, 144386.634469268, 56743.3]], [[348052.330037187, 121463.62944772957, 54625.010455681535], [350202.330037187, 125166.17907933441, 56803.88878537846]], [[345775.015446367, 114708.487025988, 54960.217194842], [352074.155704281, 116858.487025988, 57110.217194842]], [[345969.2935026183, 105741.489752301, 53531.11293542888], [351773.6030107027, 108041.489752301, 55888.07634133613]], [[346415.6796021126, 119487.224099402, 55046.210835151534], [351218.22923371743, 121637.224099402, 57243.789164848466]], [[158516.197537739, 170366.309211494, 54338.2], [163108.197537739, 172976.309211494, 56948.2]], [[158516.197537739, 170366.309211494, 54338.2], [163108.197537739, 172976.309211494, 56948.2]], [[158516.197537739, 170366.309211494, 54338.2], [163108.197537739, 172976.309211494, 56948.2]], [[158321.279657382, 167414.527694719, 54243.2], [162718.279657382, 170214.527694719, 57043.2]], [[158321.279657382, 167414.527694719, 54243.2], [162718.279657382, 170214.527694719, 57043.2]], [[158321.279657382, 167414.527694719, 54243.2], [162718.279657382, 170214.527694719, 57043.2]], [[177462.32923089, 170583.809211494, 54168.2], [181984.32923089, 173533.809211494, 57118.2]], [[177462.32923089, 170583.809211494, 54168.2], [181984.32923089, 173533.809211494, 57118.2]], [[177462.32923089, 170583.809211494, 54168.2], [181984.32923089, 173533.809211494, 57118.2]], [[177462.32923089, 170583.809211494, 54168.2], [181984.32923089, 173533.809211494, 57118.2]], [[177462.32923089, 167883.809211494, 54168.2], [181984.32923089, 170833.809211494, 57118.2]], [[177462.32923089, 167883.809211494, 54168.2], [181984.32923089, 170833.809211494, 57118.2]], [[177462.32923089, 167883.809211494, 54168.2], [181984.32923089, 170833.809211494, 57118.2]], [[177462.32923089, 167883.809211494, 54168.2], [181984.32923089, 170833.809211494, 57118.2]], [[203528.892572011, 136712.670824834, 54293.2], [211128.092572011, 139412.670824834, 56993.2]], [[203528.892572011, 136712.670824834, 54293.2], [211128.092572011, 139412.670824834, 56993.2]], [[203528.892572011, 134901.333624949, 54293.2], [212033.11782518, 137601.333624949, 56993.2]], [[203528.892572011, 134901.333624949, 54293.2], [212033.11782518, 137601.333624949, 56993.2]], [[203528.892572011, 132441.382809611, 54293.2], [210728.892572011, 135141.382809611, 56993.2]], [[203528.892572011, 132441.382809611, 54293.2], [210728.892572011, 135141.382809611, 56993.2]], [[203528.892572011, 116302.271390973, 54143.2], [213178.892572011, 119302.271390973, 57143.2]], [[203528.892572011, 116302.271390973, 54143.2], [213178.892572011, 119302.271390973, 57143.2]], [[203528.892572011, 113065.728188821, 54043.2], [211003.892572011, 116265.728188821, 57243.2]], [[203528.892572011, 113065.728188821, 54043.2], [211003.892572011, 116265.728188821, 57243.2]], [[203528.892572011, 110684.859256881, 54043.2], [210211.392572011, 113884.859256881, 57243.2]], [[203528.892572011, 110684.859256881, 54043.2], [210211.392572011, 113884.859256881, 57243.2]], [[355462.32923089, 117860.461563142, 54568.2], [359984.32923089, 120010.461563142, 56718.2]], [[355462.32923089, 118135.461563142, 54568.2], [359984.32923089, 120285.461563142, 56718.2]], [[355462.32923089, 116960.459377295, 54543.2], [359984.32923089, 119160.459377295, 56743.2]], [[355462.32923089, 116635.459377295, 54543.2], [359984.32923089, 118835.459377295, 56743.2]], [[348881.965550686, 105474.970248794, 54243.2], [351191.27933521, 108274.970248794, 57043.2]], [[346669.27933521, 105474.970248794, 54243.2], [350881.965550686, 108274.970248794, 57043.2]], [[347233.462168113, 92484.328469695, 56000.0], [350873.462168113, 94884.328469695, 58400.0]], [[345768.849995021, 98213.819295768, 55700.0], [351468.849995021, 100613.819295768, 58100.0]], [[345644.280316787, 101610.934741277, 55688.0], [351244.280316787, 104034.934741277, 58112.0]], [[347813.065319341, 110241.158927369, 54568.9], [351013.065319341, 113441.158927369, 58400.0]], [[347813.065319341, 110241.158927369, 54568.9], [351013.065319341, 113441.158927369, 58400.0]], [[346993.065319341, 119041.158927369, 54568.9], [350193.065319341, 122241.158927369, 57900.0]], [[346993.065319341, 119041.158927369, 54568.9], [350193.065319341, 122241.158927369, 57900.0]], [[84363.715770676, 100083.925494092, 51800.0], [87158.482348945, 102283.925494092, 54000.0]], [[84363.715770676, 100083.925494092, 51800.0], [87158.482348945, 102283.925494092, 54000.0]], [[84363.715770676, 100083.925494092, 51800.0], [87158.482348945, 102283.925494092, 54000.0]], [[84363.715770676, 100083.925494092, 51800.0], [87158.482348945, 102283.925494092, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[82952.002038827, 110788.457820158, 51800.0], [85272.002038827, 112988.457820158, 54000.0]], [[141277.532418336, 176570.194756174, 51430.0], [143955.015966292, 179570.194756174, 54430.0]], [[141277.532418336, 176570.194756174, 51430.0], [143955.015966292, 179570.194756174, 54430.0]], [[141277.532418336, 176570.194756174, 51430.0], [143955.015966292, 179570.194756174, 54430.0]], [[141277.532418336, 176570.194756174, 51430.0], [143955.015966292, 179570.194756174, 54430.0]], [[141277.532418336, 176570.194756174, 51430.0], [143955.015966292, 179570.194756174, 54430.0]], [[141278.620215937, 175253.968409572, 51630.0], [143955.32161717, 177853.968409572, 54230.0]], [[141278.620215937, 175253.968409572, 51630.0], [143955.32161717, 177853.968409572, 54230.0]], [[141278.620215937, 175253.968409572, 51630.0], [143955.32161717, 177853.968409572, 54230.0]], [[141278.620215937, 175253.968409572, 51630.0], [143955.32161717, 177853.968409572, 54230.0]], [[141278.620215937, 175253.968409572, 51630.0], [143955.32161717, 177853.968409572, 54230.0]], [[82354.360753559, 116796.806628537, 51630.0], [88955.986206064, 119396.806628537, 54230.0]], [[82354.360753559, 116796.806628537, 51630.0], [88955.986206064, 119396.806628537, 54230.0]], [[82354.360753559, 116796.806628537, 51630.0], [88955.986206064, 119396.806628537, 54230.0]], [[119863.375422629, 182455.955488996, 51600.0], [124863.375422629, 185055.955488996, 54200.0]], [[119863.375422629, 182455.955488996, 51600.0], [124863.375422629, 185055.955488996, 54200.0]], [[119863.375422629, 182455.955488996, 51600.0], [124863.375422629, 185055.955488996, 54200.0]], [[119863.375422629, 182455.955488996, 51600.0], [124863.375422629, 185055.955488996, 54200.0]], [[119863.375422629, 182455.955488996, 51600.0], [124863.375422629, 185055.955488996, 54200.0]], [[119863.375422629, 171888.341537928, 51600.0], [124863.375422629, 174488.341537928, 54200.0]], [[119863.375422629, 171888.341537928, 51600.0], [124863.375422629, 174488.341537928, 54200.0]], [[119863.375422629, 171888.341537928, 51600.0], [124863.375422629, 174488.341537928, 54200.0]], [[119863.375422629, 171888.341537928, 51600.0], [124863.375422629, 174488.341537928, 54200.0]], [[119863.375422629, 171888.341537928, 51600.0], [124863.375422629, 174488.341537928, 54200.0]], [[56804.601434413, 93611.498388068, 50550.0], [61504.601434413, 129593.925493724, 55250.0]], [[56804.601434413, 93611.498388068, 50550.0], [61504.601434413, 129593.925493724, 55250.0]], [[60906.865713418, 87632.917711292, 48500.0], [69706.865713418, 93921.500985707, 57300.0]], [[60906.865713418, 87632.917711292, 48500.0], [69706.865713418, 93921.500985707, 57300.0]], [[60906.865713418, 87632.917711292, 48500.0], [69706.865713418, 93921.500985707, 57300.0]], [[60906.865713418, 87632.917711292, 48500.0], [69706.865713418, 93921.500985707, 57300.0]], [[60906.865713418, 87632.917711292, 48500.0], [69706.865713418, 93921.500985707, 57300.0]], [[60906.865713418, 87632.917711292, 48500.0], [69706.865713418, 93921.500985707, 57300.0]], [[60906.865713418, 87632.917711292, 48500.0], [69706.865713418, 93921.500985707, 57300.0]]]
def pipes_to_region(p,bs):
    import numpy as np
    from numpy.core.fromnumeric import shape
    import copy
    a = []
    b = []
    c = []
    c3 = []
    c4 = []
    h1 = []
    h2 = []
    pipes = []
    blocks = []
    e1 = []
    e2 = []
    e3 = []
    area = []
    area1 = []
    area2 = []
    area3 = []
    block = []
    block1 = []
    block2 = []
    block3 = []
    temp = 10000
    for j in range(len(p)):
        p2 = p[j]
        p3 = dian2(p2[0],p2[1],p2[2],p2[3])
        pipes.append(p3)
    for i in range(len(bs)):
        b1 = bs[i]
        b2 = dian2(b1[0],b1[1],b1[2],b1[3])
        blocks.append(b2)
    a = pipes
    b=np.array(a)
    d=np.array(b.reshape(int(b.size/3),3))
    e = d.tolist()
    # print(e)
    for i in range(len(e)):
        e1.append(e[i][0])
        e2.append(e[i][1])
        e3.append(e[i][2])
    g1 = [min(e1),min(e2),min(e3)]
    g2 = [max(e1),max(e2),max(e3)]
    c.append([g1,g2])
    if c[0][0][0] > temp and c[0][0][1] > temp and c[0][0][2 ]> temp:
        area.append([[c[0][0][0]-temp,c[0][0][1]-temp,c[0][0][2]-temp],[c[0][1][0]+temp,c[0][1][1]+temp,c[0][1][2]+temp]])
    else:
        area.append([c[0][0],[c[0][1][0] + temp, c[0][1][1] + temp, c[0][1][2] + temp]])
        c.clear()
        e1.clear()
        e2.clear()
        e3.clear()
    c3 = [(area[0][1][0] + area[0][0][0]) / 2, (area[0][1][1] + area[0][0][1]) / 2, (area[0][1][2] + area[0][0][2]) / 2]
    c4 = [area[0][0], c3, area[0][1]]
    h1.extend(c4)
    h2 = point_8_to_dian2(h1)
    block3 = pzjc([h2], bs)
    for v in range(len(blocks)):
        f = copy.deepcopy(blocks[v])
        f.append([(f[0][0]+f[1][0])/2,(f[0][1]+f[1][1])/2,(f[0][2]+f[2][2])/2])
        i = 0
        while i<=8:
            if area[0][0][0] <= f[i][0] <= area[0][1][0] and area[0][0][1] <= f[i][1] <= area[0][1][1] and area[0][0][2] <= f[i][2] <= area[0][1][2]:
                if len(area1) == 0:
                    area1.append(bs[v])
                    # print(area1)

            i+=1
        block.extend(area1)
        f.clear()
        block1 = copy.deepcopy(block)
        area1.clear()
    for j in block3:
        if j not in block1:
            block1.extend(j)
    block.clear()
    block2.extend(block1)
    area.clear()
    # print("所有管道区域绑定的障碍物为",block2)
    return block2
# pipe = pipes(pipeBase)
# block = blocks()
# area = [[[169534.776042965, 171203.809211494, 55568.2], [172584.776042965, 171353.809211494, 55718.2]], [[172659.776042965, 170498.309211494, 55568.2], [172809.776042965, 171128.809211494, 55718.2]], [[172849.276042965, 170228.809211494, 55593.2], [175534.776042965, 170328.809211494, 55693.2]], [[169534.776042965, 169503.809211494, 55568.2], [172584.776042965, 169653.809211494, 55718.2]], [[172659.776042965, 168798.309211494, 55568.2], [172809.776042965, 169428.809211494, 55718.2]], [[172849.276042965, 168528.809211494, 55593.2], [175534.776042965, 168628.809211494, 55693.2]], [[169534.776042965, 167903.809211494, 55568.2], [172584.776042965, 168053.809211494, 55718.2]], [[172659.776042965, 167198.309211494, 55568.2], [172809.776042965, 167828.809211494, 55718.2]], [[172849.276042965, 166928.809211494, 55593.2], [175534.776042965, 167028.809211494, 55693.2]], [[204625.224403649, 170901.85355678, 55518.3], [209623.224403649, 171151.85355678, 55768.3]], [[209760.27822751866, 171031.84641410472, 55518.3], [211752.86202704534, 173024.4302136323, 55768.3]], [[204625.224403649, 168382.519568495, 55568.3], [212421.224403649, 168532.519568495, 55718.3]], [[212623.92808233056, 168723.03331110431, 55568.3], [212830.78240211945, 170358.97960208368, 55718.3]], [[204625.224403649, 155409.614647389, 55593.3], [209323.224403649, 155509.614647389, 55693.3]], [[204625.224403649, 152837.121221619, 55543.3], [209323.224403649, 153037.121221619, 55743.3]], [[209442.224403649, 152887.121221619, 55543.3], [211848.545069824, 152987.121221619, 55643.3]], [[204625.224403649, 148809.562387894, 55593.3], [209360.197174243, 148909.562387894, 55693.3]], [[209591.197174243, 149140.562387894, 55593.3], [209691.197174243, 149859.562387894, 55693.3]], [[209656.224403649, 155409.614647389, 55593.3], [212454.224403649, 155509.614647389, 55693.3]], [[204625.224403649, 145195.504744438, 55543.3], [207643.224403649, 145395.504744438, 55743.3]], [[208007.224403649, 145195.504744438, 55543.3], [211605.224403649, 145395.504744438, 55743.3]], [[204627.224403649, 143186.634469268, 55543.3], [211323.224403649, 143386.634469268, 55743.3]], [[349052.330037187, 122463.62944772957, 55625.010455681535], [349202.330037187, 124166.17907933441, 55803.88878537846]], [[346775.015446367, 115708.487025988, 55960.217194842], [351074.155704281, 115858.487025988, 56110.217194842]], [[346969.2935026183, 106741.489752301, 54531.11293542888], [350773.6030107027, 107041.489752301, 54888.07634133613]], [[347415.6796021126, 120487.224099402, 56046.210835151534], [350218.22923371743, 120637.224099402, 56243.789164848466]], [[159516.197537739, 171366.309211494, 55338.2], [162108.197537739, 171976.309211494, 55948.2]], [[162111.197537739, 170596.70008446, 55338.2], [162721.197537739, 171363.309211494, 55948.2]], [[162669.197537739, 170021.309211494, 55393.2], [165616.197537739, 170521.309211494, 55893.2]], [[159321.279657382, 168414.527694719, 55243.2], [161718.279657382, 169214.527694719, 56043.2]], [[161721.279657382, 168044.238372837, 55243.2], [162521.279657382, 168411.527694719, 56043.2]], [[162424.279657382, 167314.527694719, 55343.2], [164921.279657382, 167914.527694719, 55943.2]], [[178462.32923089, 171583.809211494, 55168.2], [180984.32923089, 172533.809211494, 56118.2]], [[180987.32923089, 171068.697559143, 55168.2], [181937.32923089, 171580.809211494, 56118.2]], [[181815.32923089, 170208.809211494, 55293.2], [183777.544777674, 170908.809211494, 55993.2]], [[183780.544777674, 169358.809211494, 55293.2], [184480.544777674, 170205.809211494, 55993.2]], [[178462.32923089, 168883.809211494, 55168.2], [180984.32923089, 169833.809211494, 56118.2]], [[180987.32923089, 168368.697559143, 55168.2], [181937.32923089, 168880.809211494, 56118.2]], [[181815.32923089, 167508.809211494, 55293.2], [183777.544777674, 168208.809211494, 55993.2]], [[183780.544777674, 166658.809211494, 55293.2], [184480.544777674, 167505.809211494, 55993.2]], [[204528.892572011, 137712.670824834, 55293.2], [210128.092572011, 138412.670824834, 55993.2]], [[210178.892572011, 138463.470824834, 55293.2], [210878.892572011, 140062.670824834, 55993.2]], [[204528.892572011, 135901.333624949, 55293.2], [211033.11782518, 136601.333624949, 55993.2]], [[211119.8388496647, 136142.27990260272, 55293.2], [213287.7171453113, 138310.1581982493, 55993.2]], [[204528.892572011, 133441.382809611, 55293.2], [209728.892572011, 134141.382809611, 55993.2]], [[210328.892572011, 133741.382809611, 55293.2], [213528.892572011, 134441.382809611, 55993.2]], [[204528.892572011, 117302.271390973, 55143.2], [212178.892572011, 118302.271390973, 56143.2]], [[212478.892572011, 117402.271390973, 55243.2], [215233.972572011, 118202.271390973, 56043.2]], [[204528.892572011, 114065.728188821, 55043.2], [210003.892572011, 115265.728188821, 56243.2]], [[210303.892572011, 114315.728188821, 55293.2], [215353.892572011, 115015.728188821, 55993.2]], [[204528.892572011, 111684.859256881, 55043.2], [209211.392572011, 112884.859256881, 56243.2]], [[209846.3925720109, 111684.85925688, 55043.2], [215353.89257201113, 112884.859256881, 56243.2]], [[356462.32923089, 118860.461563142, 55568.2], [358984.32923089, 119010.461563142, 55718.2]], [[356462.32923089, 119135.461563142, 55568.2], [358984.32923089, 119285.461563142, 55718.2]], [[356462.32923089, 117960.459377295, 55543.2], [358984.32923089, 118160.459377295, 55743.2]], [[356462.32923089, 117635.459377295, 55543.2], [358984.32923089, 117835.459377295, 55743.2]], [[349881.965550686, 106474.970248794, 55243.2], [350191.27933521, 107274.970248794, 56043.2]], [[347669.27933521, 106474.970248794, 55243.2], [349881.965550686, 107274.970248794, 56043.2]], [[348233.462168113, 93484.328469695, 57000.0], [349873.462168113, 93884.328469695, 57400.0]], [[346768.849995021, 99213.819295768, 56700.0], [350468.849995021, 99613.819295768, 57100.0]], [[346644.280316787, 102610.934741277, 56688.0], [350244.280316787, 103034.934741277, 57112.0]], [[348813.065319341, 111241.158927369, 55568.9], [350013.065319341, 112441.158927369, 57400.0]], [[346813.065319341, 110841.158927369, 53918.1], [348762.265319341, 112841.158927369, 55918.1]], [[347993.065319341, 120041.158927369, 55568.9], [349193.065319341, 121241.158927369, 56900.0]], [[349243.865319341, 119641.158927369, 53918.1], [350693.065319341, 121641.158927369, 55918.1]], [[85363.715770676, 101083.925494092, 52800.0], [86158.482348945, 101283.925494092, 53000.0]], [[85163.715770676, 101183.925494092, 52700.0], [85563.715770676, 103185.871211164, 53100.0]], [[85163.715770676, 103185.871211164, 52700.0], [85563.715770676, 109652.759901127, 53100.0]], [[84453.715770676, 109452.759901127, 52700.0], [85363.715770676, 109852.759901127, 53100.0]], [[83952.002038827, 111788.457820158, 52800.0], [84272.002038827, 111988.457820158, 53000.0]], [[84172.002038827, 111888.457820158, 52800.0], [84372.002038827, 115521.521754782, 53000.0]], [[84072.002038827, 115521.521754782, 52700.0], [84472.002038827, 117462.792328466, 53100.0]], [[84072.307689705, 117462.792328466, 52700.0], [84472.307689705, 119422.044860224, 53100.0]], [[84072.002038827, 119422.044860224, 52700.0], [84472.002038827, 121351.354101784, 53100.0]], [[84072.307689705, 121351.82177535, 52700.0], [84472.307689705, 123409.393167289, 53100.0]], [[84272.307689705, 123209.393167289, 52700.0], [85283.538884851, 123609.393167289, 53100.0]], [[85083.538884852, 123409.393167289, 52700.0], [85483.538884852, 124187.707997521, 53100.0]], [[142277.532418336, 177570.194756174, 52430.0], [142955.015966292, 178570.194756174, 53430.0]], [[142455.015966292, 177788.809211494, 52430.0], [143455.015966292, 178070.194756174, 53430.0]], [[142955.015966292, 177288.809211494, 52430.0], [143495.015966292, 178288.809211494, 53430.0]], [[143495.015966292, 177288.809211494, 52430.0], [144175.015966292, 178288.809211494, 53430.0]], [[143675.015966292, 177529.27688506, 52430.0], [144675.015966292, 177788.809211494, 53430.0]], [[142278.620215937, 176253.968409572, 52630.0], [142955.32161717, 176853.968409572, 53230.0]], [[142655.32161717, 176188.809211494, 52630.0], [143255.32161717, 176553.968409572, 53230.0]], [[142955.32161717, 175888.809211494, 52630.0], [143655.32161717, 176488.809211494, 53230.0]], [[143655.32161717, 175888.809211494, 52630.0], [144195.32161717, 176488.809211494, 53230.0]], [[143895.32161717, 175928.809211494, 52630.0], [144495.32161717, 176188.809211494, 53230.0]], [[83354.360753559, 117796.806628537, 52630.0], [87955.986206064, 118396.806628537, 53230.0]], [[83054.360753559, 117274.620022954, 52630.0], [83654.360753559, 118096.806628537, 53230.0]], [[82522.330897938, 116974.620022954, 52630.0], [83354.360753559, 117574.620022954, 53230.0]], [[120863.375422629, 183455.955488996, 52600.0], [123863.375422629, 184055.955488996, 53200.0]], [[123563.375422629, 182655.955488996, 52600.0], [124163.375422629, 183755.955488996, 53200.0]], [[123863.375422629, 182355.955488996, 52600.0], [128363.375422629, 182955.955488996, 53200.0]], [[126863.375422629, 182355.955488996, 52600.0], [129863.375422629, 182955.955488996, 53200.0]], [[129563.375422629, 181555.955488996, 52600.0], [130163.375422629, 182655.955488996, 53200.0]], [[120863.375422629, 172888.341537928, 52600.0], [123863.375422629, 173488.341537928, 53200.0]], [[123563.375422629, 171588.341537929, 52600.0], [124163.375422629, 173188.341537928, 53200.0]], [[123863.375422629, 171288.341537929, 52600.0], [127063.375422628, 171888.341537929, 53200.0]], [[127063.375422628, 171288.341537929, 52600.0], [129663.375422629, 171888.341537929, 53200.0]], [[129363.375422629, 170088.341537929, 52600.0], [129963.375422629, 171588.341537929, 53200.0]], [[57804.601434413, 94611.498388068, 51550.0], [60504.601434413, 128593.925493724, 54250.0]], [[59154.601434413, 127243.925493724, 51550.0], [91254.601434413, 129943.925493724, 54250.0]], [[61906.865713418, 88632.917711292, 49500.0], [68706.865713418, 92921.500985707, 56300.0]], [[64258.482348945, 85232.917711292, 49500.0], [65306.865713418, 92032.917711292, 56300.0]], [[60071.281525277, 86792.917711292, 51060.0], [64258.482348945, 90472.917711292, 54740.0]], [[54781.59494802875, 87331.84123390875, 51060.0], [61372.35800266025, 93922.60428854024, 54740.0]], [[54242.671425412, 92621.527811157, 51060.0], [57922.671425412, 130421.527811157, 54740.0]], [[54781.59494802875, 129120.45133377374, 51060.0], [61555.67791179625, 135894.53429754122, 54740.0]], [[60254.601434413, 132753.457820158, 51060.0], [90154.601434413, 136433.457820158, 54740.0]]]
#输入每段管道的区域和所有障碍物的坐标，输出管道区域和区域内的障碍物
def pipe_and_block_to_area(area,block):
    import numpy as np
    from numpy.core.fromnumeric import shape
    import copy
    area1 = []
    b = []
    c = []
    block1 = []
    block2 = []
    block3 = []
    blocks = []
    for i in range(len(block)):
        b1 = block[i]
        b2 = dian2(b1[0],b1[1],b1[2],b1[3])
        blocks.append(b2)
    for i in range(len(area)):
        a = area[i]
        # print(a)
        for v in range(len(blocks)):
            f = copy.deepcopy(blocks[v])
            f.append([(f[0][0]+f[1][0])/2,(f[0][1]+f[1][1])/2,(f[0][2]+f[2][2])/2])
            # print(f)
            i = 0
            while i<=8:
                if a[0][0] <= f[i][0] <= a[1][0] and a[0][1] <= f[i][1] <= a[1][1] and a[0][2] <= f[i][2] <= a[1][2]:
                    if len(area1) == 0:
                        area1.append(blocks[v])
                        # print(area1)
                i += 1
            block1.extend(area1)
            f.clear()
            block2 = copy.deepcopy(block1)
            area1.clear()
        block1.clear()
        block3.append(block2)
        a.clear()
    # print(block2)

# blockBase= BlockClass.BlockBase('C:/Users/Administrator/Desktop/bysj/pythondm/Input test 01-20220416A.xml');
# list = [[[190499.494283648, 169090.192301657, 54900.0], [191299.494283648, 169090.192301657, 54900.0], 800.0, 4000.0], [[191821.201035488, 169090.192301657, 54900.0], [192821.201035488, 169090.192301657, 54900.0], 1000.0, 4000.0], [[189853.725801005, 167165.10170667, 55862.5], [194653.725801005, 167165.10170667, 55862.5], 1600.0, 75.0]]
def check(list,blockBase):
    # typelist = []
    for one in list:
        for block in blockBase._blocks:
            startPoint = one[0];
            endPoint = one[1];
            if (operator.eq(startPoint, block.start) and operator.eq(endPoint, block.end)):
                return block.type
                # return typelist[0]

def checkID(one,blockBase):
    # typelist = []
    for block in blockBase._blocks:
        startPoint = one[0];
        endPoint = one[1];
        if (operator.eq(startPoint, block.start) and operator.eq(endPoint, block.end)):
            print("匹配障碍物uid:",block.uid)
            return block.uid
            # return typelist[0]

def checkPipeID(one,pipeBase):
    startPoint = one[0];
    endPoint = one[1];
    for pipeGroup in pipeBase._pipeGroups:
        for pipeList in pipeGroup._pipeLists:
            for pipe in pipeList._pipes:
                if (operator.eq(startPoint, pipe.start) and operator.eq(endPoint, pipe.end)):
                    print("匹配管道uid:",pipe.uid)
                    return pipe.uid
    return "没有找到管道uid."


# pipe_and_block_to_area(area,block)

# 障碍物对象解析成数组
def blocks_to_arr(blockBase):
   array=[]
   for block in blockBase._blocks:
      array.append([block.start,block.end,block.size.width,block.size.height])
   return array

# check(list,blockBase)